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Abstract: The integrated navigation system with strapdown inertial navigation system 
(SINS), Beidou (BD) receiver and Doppler velocity log (DVL) can be used in marine 
applications owing to the fact that the redundant and complementary information from 
different sensors can markedly improve the system accuracy. However, the existence of 
multisensor asynchrony will introduce errors into the system. In order to deal with the 
problem, conventionally the sampling interval is subdivided, which increases the 
computational complexity. In this paper, an innovative integrated navigation algorithm 
based on a Cubature Kalman filter (CKF) is proposed correspondingly. A nonlinear system 
model and observation model for the SINS/BD/DVL integrated system are established to 
more accurately describe the system. By taking multi-sensor asynchronization into account, 
a new sampling principle is proposed to make the best use of each sensor's information. 
Further, CKF is introduced in this new algorithm to enable the improvement of the filtering 
accuracy. The performance of this new algorithm has been examined through numerical 
simulations. The results have shown that the positional error can be effectively reduced 
with the new integrated navigation algorithm. Compared with the traditional algorithm 
based on EKF, the accuracy of the SINS/BD/DVL integrated navigation system is 
improved, making the proposed nonlinear integrated navigation algorithm feasible 
and efficient. 
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1. Introduction 

In modern marine navigation, the strapdown inertial navigation systems (SINS) is widely used due 
to its advantages of being more compact and autonomous. However, accumulated navigation errors are 
inevitable in SINS and may become considerably conspicuous in the long-term. Consequently, it is 
often aided with other sensors, e.g., global positioning system (GPS) and Doppler velocity log (DVL) 
etc. The accuracy of the integrated system can thus be effectively improved owing to the redundancy 
and complementarity of the measurements [1-3]. Nowadays, the GPS-aided SINS integrated system is 
the most popular marine navigation system. Besides GPS, GLONASS, Gallileo, and another satellite 
navigation system named Beidou (BD) is being developed, which can provide precise position 
information via the double-star positioning theory [4,5]. This study focuses on the SINS/BD integrated 
system and further integrates DVL into the SINS/BD system to maintain and improve the system 
accuracy in poor BD or BD denied environments [5,6]. 

One outstanding feature of BD is that it is an active inquiry -response positioning system. The user's 
position information is sent to the ground central control system through two satellites and then 
processed by the ground central control system. Then, the processed information is sent back to 
the satellites, and finally the estimated user's position is sent to the user by the satellites [7,8]. 
Accordingly, the signals are transmitted multiple times between the ground receiver and satellites. 
With the additional processing time of the calculation center, time-delays appear in the user's position. 
This causes the asynchronous phenomenon in a SINS/BD/DVL integrated navigation system, which 
will degrade the accuracy of the system. Therefore, an advanced asynchronous algorithm with small 
computational cost and high accuracy is important for SINS/BD/DVL integrated navigation. 

To solve the multi-sensor asynchronous problem, a SINS/Beidou/STAR integrated navigation 
system based on the federal filtering algorithm was built up [9]. Prior delayed information was 
recorded to correct the estimated states and their covariance matrix. In [10] an algorithm of weighted 
co variance for centralized asynchronous fusion (WCCAF), which fused the latest predicted state vector 
with the existing estimated state vector was proposed. The simulation results showed that the maximal 
position RMSE was 6 m in 90 s with the proposed method. Although these two methods could dampen 
the estimation error due to the asynchronization among multiple sensors, both of them are based on 
Kalman filters, so they are only suitable for linear systems. Since almost all actual systems are 
nonlinear, nonlinear filters should be used for multi-sensor information fusion [11-18]. In [11] an 
information fusion algorithm based on the Extend Kalman Filter (EKF) was introduced to solve 
nonlinear problems in multi-sensor integrated navigation, but the precision is limited because of the 
Taylor expansion and the EKF needs to calculate the fussy Jacobian matrix which increases the 
computational load. With the presented algorithm, 80% of errors in estimation are within 16 m in 50 s. 
The authors of [18] proposed an integrated navigation algorithm based on Unscented Kalman Filter 
(UKF) which was applied to a SINS/CNS (Celestial Navigation System)/GPS integrated system. 
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In [18], the local UKF was used to estimate the nonlinear integrated system and the federated Kalman 
filter was used to fuse the predictions of local filters, but in high-dimensional systems, the computation 
load is still heavy, thus, the filter converges slowly. In 2009, Arasaratnam and Hay kin [19] proposed a 
more accurate nonlinear filtering solution based on a Cubature transform named Cubature Kalman 
filter (CKF) which can avoid linearization of the nonlinear system by using Cubature point sets to 
approximate the mean and variance. The third-order accuracy of the system can be achieved with this 
method. Because of its high accuracy and low calculation load, the CKF is widely used in attitude 
estimation and navigation [20-22]. 

In this paper a novel asynchronous algorithm for the SINS/BD/DVL integrated navigation system is 
proposed on the basis of CKF. Meantime, new nonlinear system and measurement models are also 
established for the measurements from SINS, BD and DVL. Taking multi-sensor asynchronization into 
account, a new sampling principle is proposed to make the best use of individual measurements. Even 
better, CKF can not only reduce the computational complexity, but also improve the accuracy of the 
navigation solution. The results from simulations showed that the proposed algorithm is superior to the 
conventional one. The rest of the paper is organized as follows. The description of the error differential 
equations of the SINS/BD/DVL integrated navigation system and the nonlinear filter named CKF are 
presented in Section 2. Section 3 shows the new sampling principle and the new asynchronous 
integrated navigation algorithm. Numerical examples along with specfic analysis are given in Section 4. 
Section 5 concludes this manuscript. 

2. Sensor Error Models and Nonlinear CKF 

2.7. Nonlinear Error Model of SINS 

Traditional linear differential equations are obtained under the assumption that the misalignment 
angles are small, so modeling errors are inevitable due to the nonlinearity of the true error model [3]. 
To improve the accuracy of the system model, a nonlinear error model of large azimuth misalignment 
angle for SINS is considered in this paper. 

In this paper, / , b , e , n and n' denote the inertial coordinate system, the body coordinate system, 

the earth coordinate system, the navigation coordinate system, and the calculation coordinate system of 
SINS, respectively. Suppose that n can be transformed to n' by turning </> z , (j> x and cj) y successively, 

wherein <p = ^ $ y (j) z ~J is the Euler error angle vector, the direction cosine matrix from n to n f is . 

Using s(j> n cfa (i = x, y 9 z) denote sin(^.) and cos ), respectively, can be describled as follows: 



C n = 



C(j) y C(j) z - S(j) y S(j) x S(j) z C(j) y S(j) z + S(j) y S(j) x C(j) z S(f) y C(l) x 

-c<f> x s<f> z c<f> x c<f> z s<f> x 

S(j) y C(j) z + C(j) y S(j) x S(j) z S(j) y S(j) z - C(j) y S(j) x C(j) z C(j) y C(j) x 



(1) 



The nonlinear attitude error equation of SINS can be derived as follows: 

+ = c-J [(/ - c;' ) d>: + c:Sco: - c;V ] + c-Jc?w g b (2) 
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wherein denotes the direction cosine matrix from b to n f , s b and are the gyro constant drift 
vector and the zero-mean Gaussian white noise vector, respectively, d>" n is the gyro measurement 
vector, a)? is the rotating angular rate vector of n relative to i , Sco" n is the calculated error vector of 
co? n . The gyro measurement vector is equal to =coT + Sojh n . C a is an intermediate matrix as follows: 



cos </> 0 - sin </> cos </> x 

0 1 sin 0 x 
sin ^ 0 cos 6 cos ^ 



(3) 



The SINS velocity error equation is given by: 

Sv n =C:'f b -Cff +C n b Sf b -{2Sa£ +^Jx(v w -^v w )-(2^ +^)x^ 



(4) 



wherein f b and Sf b denote the specific force vector and its corresponding error vector respectively, 
6f ie is the calculated Earth's rotating angular rate, df en is the calculated angular rate vector, 8co n ie and 
8co n en indicate the error vectors of 6f ie and af en respectively, v n and Sv n denote velocity measurement 
vector and its corresponding error vector, 8g n is the gravity acceleration error, and Q n = C",Cf . 

Suppose that Sf b is composed of the constant bias error and the zero-mean Gaussian white 
noise vector W b . If 8g n is ignored, Equation (4) can be rewritten as follows: 



8v n = C n b f b - C n b f b + C n b V b - (2Sc% + &£ ) x (v n - Sv n ) - {2a? ie + ^ ) x ^ + cx' 



(5) 



Because both of the gyro and accelerometer errors are composed of a constant error vector and a 
zero-mean Gaussian white noise vector, their differential equations are: 

l>=0 

\v b =o 

The position error equations comprise the longitude error SX and the latitude error 8q> : 



(6) 



8X = 8(p tan <p sec ^ — + sec ^ 



(7) 



wherein 7? M and /? w are the Earth's radii of the meridian circle and the prime vertical circle, 
respectively; X and q> are the longitude and latitude of a point of interest; v x and v are the east and 
north velocities with their velocity errors 8v x and 8v , respectively. 



2.2. Error Model ofBD 

The location information can be received directly from BD. The major error sources which affect the 
measurement accuracy of BD are the error of the BD receiver, the track error and the multi-path effect. 
To focus on the asynchronicity problem of multi-sensor systems, only the clock error of a BD receiver 
is taken into account here, including the clock bias and the clock frequency drift [6]. Despite the fact 
that the clock bias consists of constant and random components, only the constant bias is taken into 
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account here for simplification. Normally, one uses At and 8 t to denote the clock constant bias and 
the clock frequency drift. So the shaping filter of the BD receiver's clock error is described as follows: 



\Ai = S t 

\S=-rAt + W x 



wherein r is the correlation time and W x is the white noise. 



(8) 



23. Error Model ofDVL 

The DVL functions as a sensor that measures the frequency shift of an acoustic signal, either 
transmitted or received by a moving object, which is proportional to the velocity of the moving 
object [2,23]. It can not only provide high accuracy absolute velocity, but aslo have satisfactory 
anti-interference performance, hence, DVL is widely deployed in marine navigation systems. The 
working principle of a DVL is based on the Doppler effect and the principle is described in Figure 1 . 

Figure 1. The schematic of the velocity errors measured by the DVL. 

N f 
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► 



(10) 



In Figure 1, K means the true heading, K d is the heading with the drift angle A, the drift of the 
angle error is denoted by SA , and a z indicates the azimuth misalignment angle. By using V d to 
denotes the velocity vector measured by the DVL, the following velocity equations are satisfied: 

v;=(i+sc)(v d +sv d ) (9 ) 

\V dx =(l + SC)(V d +SV d )sm(K d +a z +SA) 
[v; y ={l + SC)(V d +SV d ) cos {K d +a z +SA) 

where SC indicates the scale factor error, V d and SV d denote the true velocity vector and the velocity 
drift error vector, respectively. V dx and V dy are the components of V d . Since both a z and £A are small 

enough, the Equation (10) can be rewritten as follows: 

\VL = y d sin K o + y d sin K d {a z+ SA) + SC-V d sin K d + SV d sin K d 
\Vj y = V d cos K d -V d sin K d ■ (a z + SA) + SC-V d cos K d + SV d cos K d 



(11) 



From Figure 1, one can also obtain: 
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\v x =V d smK d 

jv^cos*, (12) 

According to the working principle of the DVL, one can obtain the velocity and the drift angle 
relative to the seafloor. Thus, the measurement errors include the velocity drift error SV d , the scale 
factor error SC and the drift angle error SA [2,4]. The DVL error model is as follows: 

w d =-p d sv d +w d 

SA = -J3 A SA + W A (13) 
SC = 0 

where , /J" 1 denote the correlation times of SV d and SA respectively; W d , W A are the 
corresponding white noises. 

2.4. Cubature Kalman Filter 

Consider the following discrete-time nonlinear state-space model: 

\xk=f(*k-i) + Wk-i 
z k =h{x k ) + rj k 

wherein x k and z k are the state vector and the measurement vector at time k , respectively; /(•) and 
/*(•) are specific known nonlinear functions; and W k _ x and rj k are the noise vectors from two 
independent zero-mean Gaussian processes with their covariance matrices Q k _ x and R k , respectively. 

CKF is proposed to solve this nonlinear filtering problem on the basis of the spherical-radial 
cubature criterion. CKF first approximates the mean and variance of probability distribution through a 
set of 2N (N is the dimension of the nonlinear system) Cubature points with the same weight, 
propagates the above cubature points through the nonlinear functions, and then calculates the mean and 
variance of the current approximate Gaussian distribution by the propagated cubature points [19]. 

A set of 2N Cubature points are given by [<^ ,&> ] , where is the i-th cubature point and co i is the 

corresponding weight: 

1 (15) 

co. = — 
1 2N 

wherein / = 1, 2,...2N . 

Under the assumption that the posterior density at time k-1 is known, the steps involved in the time- 
update and the measurement-update of CKF are summarized as follows [19]: 
Time-update: 

P = S S 1 

1 k-i\k-i ^fc-iijfc-i^jfc-iifc-i 



^i,k-l\k-l ^k-l\k-l^i + *k-l\k-l 
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x tk\k-i ~ f( X i,k-i\k-i) 



1 2N 

=—Yx' 

O AT l > k \ k 



k\k-l 



2Nfi 



1 IN 

P =_Vy* Y* r -r r T +f) 

1 k\k-i 2N ^ iA k ~ l l A k - 1 k \ k ~ l k \ k ~ l ^ k ~ l 



Measurement-update : 



P = V V 7 

j IN 

yk\k-\ = ^t7X^a-i 



1 27V 

pzz _ 1 y yr - ~T p 

r k\k-l 2J\[ ^ i,k\ k ~ l Uk\k-l yk\k-Wk\k-l k 

i 2N 

p xz = — — V y y T -r v T 

1 k\k-\ 0 AT i,k\k-l ± i,k\k-\ ^k-W k\k-\ 



^ L y i=l 

With the new measurement vector z k , the estimated of the state vector x k ^ k and its covariance 
matrix P k ^ k at time k can be obtained by the following equations: 

— p xz ( p zz \ 1 
A A _ r k\k-\ \ r k\k-\ ) 

K\k = K\k-\ +K k[ z k~ z-k\k-\ ) 
^k\k ~ ^k\k-\ ~^k^k\k-i^k 

wherein K k is the filter gain at time k . 

CKF uses cubature rule and 2N cubature point sets [<^ ,&> ] to compute the mean and variance of 

probability distribution without any linearization of a nonlinear model. Thus, the modeling can reach 
the third-order or higher. Furthermore, this filtering solution does not demand Jacobians and Hessians 
so that the computational complexity will be alleviated to a certain extent. 

3. Novel Nonlinear Integration Algorithm for Nonlinear SINS/BD/DVL Based on CKF 

3.1. Nonlinear Model of SINS/BD/DVL 

The nonlinear model for a SINS/BD/DVL integrated navigation system is established under the 

large azimuth misalignment angle in this paper. Considering the following error states: the longitude 
error 81 , the latitude error Sep , the east velocity error Sv x , the north velocity error Sv , the Euler 

angle errors ^ and ^, the accelerometer zero-biases V x , V , the constant gyro drifts s x , e y , s z \ 
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the clock constant bias At and the clock frequency drift 8 t of the BD clock error, the velocity drift 
error SV d , the scale factor error SC and the drift angle error SA of DVL, the state vector is built up 
as follows: 

X=[S<p SA Sv x 8v y <t> x (f> y </> z V x V y s x s y e z At S t 5V d SA ScJ 

The corresponding state equation is written as: 

^=/W+^ H (16) 
The state function /(•) can be obtained from Equations (1)— (13) and [3]. Futher, the process noise 
vector is given by: 

w = [o lx2 w„ w ay w gx w gz o lx6 w s w d w A oj 

wherein W nv and W are the white noises of accelerometer; W , W and W are the white noises of 

ax ay ' gx ' gy gz 

gyroscopes drifts; W s is the white noise; W d , W A are the white noises of 5V d and ^A, respectively. 

To solve the problem of asynchronism, a new method is proposed to establish the measurement 
equations. The multi-sensor measurements can be pre-processed separately. Then, the central fusion 
blends all of the pre-processed data to obtain the optimal state vector. Here, the measurements are 
divided into two groups: pseudo-ranges and pseudo-range rates as the measurements for the SINS/BD 
filter, and the velocity errors as measurements for the SINS/DVL filter. 

The measurement equation for the SINS/BD filter is [8]: 

5p i = S(p^R N {-e a sin^cosyL-sin^sinyL) + ^. 3 /? ;v (l-/) 2 cos^j 

+ SA^R N [e i2 cos^cos/l-^ cos<^sin/l)) + v c -At + r/ lip (17) 
5p i = 5v x {—e a sin X + e i2 cos X) + Sv {—e a sin <p cos X - e i2 sin X cos <p + e i3 cos cp) + v c • 5 + r/ U/ ^ 

wherein i=l,2,3,4 is the number of satellites, 8p i and 8p { are the pseudo-range residual and the 
pseudo-range rate residual between SINS and BD receiver, respectively, v c is the velocity of light, 
e iV e i29 e i3 are the direction cosine from the user to the i-th satellite, rj Xi and rj lip are the measurement 

noise vectors. 

The velocity error measurements between the SINS and the DVL are as follows: 

\ Z 2,x = ~ 5V x + V yA + V y SA + V x 5C + SV d ^ K d + %,x 

[Z Xy = Sv y + vj z + v x SA -v y SC- SV d cos K d +^ y ( 1 8) 

wherein rj 2x , r/ 2y are the DVL measurement noises. 

3.2. Nonlinear Integration Navigation Algorithm Based on CKF 

In this subsection, a CKF-based novel nonlinear algorithm is structured to solve the asynchronicity 
problem. In general, the smaller the sampling interval one uses, the higher system accuracy one can 
achieved, but accompanied with a larger calculation burden. A proper sampling interval should be 
designed accordingly. Now, a new sampling principle is presented. If the sampling interval of SINS, 
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BD and DVL are 7j , T 2 and T 3 respectively, the greatest common divisor (GCD) of T x , T 2 and T 3 is 
denoted as GCD(T 19 T 2 ,T 3 ) . Thus, the sampling interval of the integrated navigation system AT is set 
as below: 

AT= rmn(T v T 2 J 3 ) 



GCD(T V T 2 ,T 3 ) 

Using this sampling criterion AT is the maximal sampling interval which can sample all of sensors' 
measurements. So the system accuracy can be improved without the expense of calculation burden. 
The sampling principle of the SINS/BD/DVL integrated navigation system is described as Figure 2. 

Figure 2. The sampling principle of SINS/BD/DVL integrated navigation system. 

I I I I I I i7ji I I I I I at this time received BDs measures, 
— — 1 1 — 1 1 — I ► I process SINS /BD system 



T 7 I I at this time received DVLs measures, 

— — I 1 



process SINS I DVL system 

jjyj^ q a t this time received BDs ana 

I process SINS/BD/DVL system 



T 

3 I I I m/r * at this time received BDs and DVLs measures, 



f /n t /n I^^L f t Integrated at this time only SINS' s information, 

* * * * * * * * ^ ► System ^ process SINS system 

(a) If only the measurements from SINS and BD are available at time k , the local SINS/BD states 
can directly be estimated using CKF. For more details on this please refer to Section 2.2. Based 
on the locally estimated state vector X { and its covariance matrix P x , the state vector of the 
SINS/BD/DVL integrated navigation system at time k can be estimated as follows: 

X f,k = X u 

(b) Similarly, if only the measurements from SINS and DVL are available at time k , the local 
SINS/DVL states are also directly estimated using CKF. From the locally estimated state vector 
X 2 and its covariance matrix P 2 , the state vector of the SINS/BD/DVL integrated navigation 
system at time k can be determined by 

X f,k = X 2,k 

(c) If all measurements from SINS, BD and DVL are available at time k , the local states for 
SINS/BD and SINS/DVL are estimated using their own CKF respectively. Thus, one can first 
estimate the local state vectors X l , X 2 and their covariance matrixes P x , P 2 , and then combine 

the locally estimated state vectors by sensor nodes: 

X f:k =DX+D 2 X 2 

wherein D x and D 2 are the corresponding weighting matrices for both of the subsystem: 

SINS/BD and SINS/DVL. Suppose that the sensors are independent, the individual suboptimal 
estimations of the state vectors can be obtained. After the minimum variance principle, the 
weighting matrices can be determined, which is explained in details in [12]. Finally, the 
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weighted state vector of the SINS/BD/DVL integrated navigation system at time k is 
deduced as: 

2 

X f,k = P f,k^j P f,k X i,k 



i=\ 



with: 



p 1 = y p 1 



(d) If no measurement is available at time k , the time-update can be performed to predict the state 
vector from the previous time. Thus, the state vector of the SINS/BD/DVL integrated 
navigation system is: 

X fk =x(k\k-l) 
Figure 3 illustrates the proposed nonlinear algorithm based on CKF. 

Figure 3. Flow chart of novel algorithm based on CKF. 




/Check whether the measurements of BD*s and DVL' 
*v are valid or not. 



BD 

measurements 
valid 



I C None of 
measurements . ... 

... I I them valid , 

valid J J 



IE 



(\ SINS/Bm/^INS/BD/DVLYSINS/DvO 
vCKF filterj I CKF filter J I CKF filter 



only time 
up-date 



State X f and covariance matrix P f 



The solution accuracy of the SINS/BD/DVL integrated navigation system can be improved 
enormously via CKF whilst the asynchronous problem is solved by this method. Besides, the 
computational cost of the BD control system of the ground center can also be reduced by using this 
sampling principle. 
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4. Simulations and Results 

Simulations were performed in this work. Their results are presented in this section. Suppose that 
the swing dynamic model of a marine vehicle is given by: 

'y/ = y/ m sm(cD ¥ t) + y/ k 

<0 = G m sm(a) e t) + G k 

where 0 , y and y/ are pitch, roll and yaw angles, respectively; the swing amplitudes were set up as 
0 m = 5° , y m = 3° , and y/ m = 8° ; the swing periods were T 0 = 8 s , T y = 6 s , 7^ = 10 s ; and the initial 
attitudes were 0 k = y k = 0° , y/ k = 30° . The vehicle's motion states are listed in Table 1. The total time 
of each simulation was 10,800 s, and the sailing track of the vehicle is shown as in Figure 4. 



Table 1. Motion states of the marine vehicle. 



Motions States 


Time (s) 


Acceleration {mis 1 ) 


1 . Mooring 


0-300 


a x - ciy = 0 


2. Accelerated motion 


300-620 


a x = 0.025, ay = 0.035 


3. Uniform motion 


620-1,620 


a x = ciy = 0 


4. Accelerated motion 


1,620-2,100 


a x = -0.04, ay = 0.005 


5. Uniform motion 


2,100-3,100 


a x = a x = 0 


6. Accelerated motion 


3,100-3,700 


a x = 0.007, a,, = -0.035 


7. Uniform motion 


3,700-5,200 


a x - ciy = 0 


8. Accelerated motion 


5,200-6,200 


a x = 0.018, Oy = 0.015 


9. Uniform motion 


6,200-10,800 


a x = a y = 0 



Figure 4. Sailing track of the marine vehicle. 
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The initial conditions of different sensors are presented as follows: 

(1) The initial latitude and longitude: = 45.7796°, 2 = 126.6705° ; their errors: Sep = 0.5° , 
^ = 0.5°; 

(2) The initial velocity components: v x = 0, v y = 0 ; their errors: 8v x = 0.8 ml s , Sv = 0.8 m/ s\ 

(3) The acceleration due to the gravity: g 0 = 9.7805 ml s 2 \ 

(4) The initial misalignment angles: 0 x = 1°, (f> = 1°, cj) z = 5° ; 

(5) The SINS gyro constant drifts: s x = s y = s z = 0.01° / h ; 

(6) The SINS gyro random noises: W gx = W gy = W gz = 0.005° / h ; 

(7) The SINS accelerometer constant biases: V x = V y = 10" 4 g 0 ; 

(8) The SINS accelerometer random noises: W ax = W ay = 10" 5 g 0 ; 

(9) The constant bias of the BD clock error: At = 30 m ; 

(10) The frequency drift of the BD clock error: S t = 0.01 ml s ; 

(11) The correlation time: r = 30 min ; 

( 1 2) The D VL velocity drift error: SV d = 0.05 mis; 

(13) The DVL scale factor error: 8C = 10^; 

(14) The DVL drift angle error: SA = V ; 

(15) The correlation times of SV d and £A : fi d l = 5 mm, fi^ =15 min . 

Under the same simulation conditions, the nonlinear algorithm based on CKF was used to estimate 
state vectors for the SINS/BD/DVL integrated navigation system. The solution was compared with the 
CKF solution only using the measurements from SINS/BDor from SINS/DVL. Assume the sampling 
intervals of BD and DVL are 0.5 s and 0.1 s, respectively, while the sampling interval of the fusion 
center is 0.05 s. First, the alignment lasted 15 min, and then the navigation was performed. The 
simulation results are presented in Figure 5 and Table 2. Here the north position error, the east position 
error and the position error are used to describe the performance of the simulation results in which the 
location error is as follows: 

position error = ^[north position error) 2 + (east position error) 2 

Figure 5 and Table 2 show that the north position error, the east position error and the position error 
from the SINS/BD/DVL integrated solution were much smaller than the errors from the subsystems: 
SINS/BD and SINS/DVL respectively. Besides, the position error converged rapidly with the proposed 
algorithm. By using the redundant and complementary measurements from the SINS/BD/DVL 
integrated navigation system, the novel algorithm can reduce the impact of the asynchronous problem. 
Thus, the position error can be decreased availably, and the navigation accuracy can be increased 
significantly. Since it was assumed that all sensors were independent in this research, the estimation 
results were suboptimal. The equipment errors, such as the gyro drifts, the accelerometer biases, and 
the misalignment angles, can also bring errors to the navigation solution. Considering the above 
reasons, the delivered results are acceptable and reasonable. 
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Table 2. Simulation Results with different sensor data. 



Different Sensor Data 




Maximal Errors (m) 




North Position Error 


East Position Error 


Position Error 


SINS/BD 


275.1 


-183.4 


219.8 


SINS/DVL 


-314.5 


-185.9 


202.3 


SINS/BD/DVL 


-118.6 


-98.7 


109.1 



Figure 5. (a) The north and east position errors compared with the ones from the individual 
subsystems; (b) The position errors compared with the ones from the individual subsystems. 
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To prove the superiority of the proposed nonlinear asynchronous fusion algorithm based on CKF, 
another simulation was carried out with the traditional fusion algorithm based on EKF introduced 
in [11]. The simulation conditions were the same as indicated above. The simulation results are given 
in Figure 6 and Table 3. 



Figure 6. (a) The north and east position errors compared with the ones from the traditional 
algorithm; (b) The position errors compared with the ones from the traditional algorithm. 



400 




10000 



12000 




Sensors 2014, 14 



1525 



Table 3. Simulation results with different filters. 


Different Filters 


Maximal Errors (m) 




North Position Error East Position Error 


Position Error 


EKF 


-384.4 -255 


284.5 


CKF 


-118.6 -98.7 


109.1 



As can be seen from Figure 6 and Table 3, compared with the traditional nonlinear fusion method 
based on EKF, the north position error, the east position error and the position error of the 
SINS/BD/DVL integrated navigation system are smaller with the new algorithm based on CKF. With 
the traditional method based on EKF the maximal position error was about 284 m as the one with the 
proposed integration algorithm was nearly 109 m. That is, the position error was decreased by 61.6%. 
As CKF uses cubature rule and 2N cubature point sets [<^ ,&> ] to compute the mean and variance of 

probability distribution without any linearization of a nonlinear model, the filtering accuracy can be 
improved significantly. Hence, the higher navigation accuracy can be obtained based on CKF. 

5. Conclusions 

In this manuscript, a novel nonlinear integrated navigation algorithm based on CKF was proposed 
in order to solve the multi-sensor asynchronicitybproblem and reduce the high calculation load of the 
SINS/BD/DVL integrated navigation system. The main focus of this work was on establishing of a 
nonlinear system model and proposing of a new sampling principle to take multi-sensor asynchronism 
into account. The superiority of CKF was analyzed theoretically for the situation with the nonlinear 
system and measurement models. To verify the new navigation algorithm, numerical simulations were 
carried out. The results showed that the proposed nonlinear fusion algorithm based on CKF cannot 
only solve the asynchronicity problem of the SINS/BD/DVL integrated navigation system, but also 
significantly improve the navigation accuracy of the nonlinear system without imposing any additional 
calculation burden. However, under the assumption made in this study that all sensors in the integrated 
system were independent, the fusion results were suboptimal. Our future work will focus on a fusion 
algorithm that is suitable for multi-sensor asynchronous systems with the correlated noises. 
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